#include <cusp/linear_operator.h>
#include <cusp/transpose.h>
#include <cusp/gallery/eye.h>
#include <cusp/array1d.h>
#include <cusp/copy.h>
#include <cusp/print.h>
#include "math.h"
#include "cublas.h"

// Version of QR with Householder reflectors
// The output is Q and the *transpose* of R (this is easier to program with contiguous access)


namespace cusp
{

template <typename LinearOperator,
          typename Matrix1,
          typename Matrix2>
__host__ 
void qrhhgemv(LinearOperator& A,
        Matrix1& Q,
        Matrix2& R)
{

  // Assumption:  all 2D arrays in column-major ordering!

  CUSP_PROFILE_SCOPED();

  typedef typename LinearOperator::value_type   ValueType;
  typedef typename LinearOperator::memory_space MemorySpace;

  typedef typename cusp::array1d<ValueType, MemorySpace>::iterator Iterator;

  R = A;
  size_t m = A.num_rows;
  size_t n = A.num_cols;
  size_t lda = A.pitch;

  // std::cout << " m " << m << " n " << n << " lda " << lda << "\n";
  cusp::gallery::eye(Q, m, m);
  size_t ldq  = Q.pitch;

  for (size_t j = 0; j < n; ++j)
  {
    cusp::array1d<ValueType, MemorySpace> uj(m-j);
    cusp::array1d<ValueType, MemorySpace> y(n-j);
    cusp::array1d<ValueType, MemorySpace> z(m);

    // pitch is generally = num_cols, but it may be contain buffer space?
    thrust::copy(R.values.begin() + j*lda+j, R.values.begin()+j*lda+m, uj.begin());

    ValueType nuj = cusp::blas::nrm2(uj);

    if (nuj == 0) cusp::print(uj);  // null vector; algorithm must terminate softly
    if (uj[0] < 0) 
	uj[0] -= nuj;
    else
        uj[0] += nuj;
    nuj = cusp::blas::nrm2(uj);
    cusp::blas::scal(uj,1/nuj);

    // cusp::print(Rt);
    // cusp::print(R.values);
     // A update:  This seems to be the unintuitive but correct formulation
    cublasSgemv('t', m-j, n-j, ((float)1.0), thrust::raw_pointer_cast(&R.values[j+j*lda]), lda, thrust::raw_pointer_cast(&uj[0]), 1, ((float)0.0), thrust::raw_pointer_cast(&y[0]), 1);  
    cublasSger(m-j, n-j, -((float)2.0), thrust::raw_pointer_cast(&uj[0]), 1, thrust::raw_pointer_cast(&y[0]), 1, thrust::raw_pointer_cast(&R.values[j+j*lda]), lda );

    // Q update:  Again, unintuitive, but seems to give correct answers
    cublasSgemv('n', m, m-j, ((float)1.0), thrust::raw_pointer_cast(&Q.values[j*ldq]), ldq, thrust::raw_pointer_cast(&uj[0]), 1, ((float)0.0), thrust::raw_pointer_cast(&z[0]), 1);
    cublasSger(m, m-j, -((float)2.0), thrust::raw_pointer_cast(&z[0]), 1, thrust::raw_pointer_cast(&uj[0]), 1, thrust::raw_pointer_cast(&Q.values[j*ldq]), ldq );
    // cusp::print(Q);

  }
  // cusp::print(Rt);

  CUSP_PROFILE_DUMP();
}

} // end namespace cusp

